/*
	--== Worm Nudger ==--
	
	(c) 2010 by Bj?rn Weber and Wagner Steuer Costa
	Version 0.1
	
	Arduino program for triggering a servo that nudges a shell
	full of caenorhabditis elegans worms to stimulate them.

	
	This program implements only the first mode of WormNudger
	
	
	Every mode triggers an impuls for the servo, that nudges the shell with the worms. 
	Furthermore with every impuls a digital output signal is generated, that corresponds to the servo impuls.
	The mode is adjustable by a button, just as the interval of the automatic mode is by two buttons. 
	A LCD displays status information while operation and gives a feedback of the button presses.
	

	In the standard configuration the digital pins are assigned as follows:
	
	00: TTLOut - digital output, triggered everytime the serve nudges the worms
	01: TTLIn - digital input, in mode 2, starts the repeated nudging of the worms
	02: LCD pin
	03: LCD pin
	04: LCD pin
	05: LCD pin
	06: StartPin - digital input, in mode 4, triggers a servo impuls to nudge the worms
	07: ModePin - button, which sets the operation mode
	08: PlusPin - button, which increases the interval time
	09: MinusPin - button, which decreases the interval time
	10: FootPedal - button (foot pedal), in mode 3, triggers a servo impuls to nudge the worms
	11: LCD pin
	12: LCD pin
	13: ServoPin - the servo is connected to this pin
*/

// include needed libraries
#include <Servo.h>
#include <LiquidCrystal.h>
#include <stdio.h>

// setup the impuls output pin
const int TTLOut 		= 0;

// setup the impuls start trigger input pin (for mode 2)
const int TTLIn 		= 1;

// initialize the lcd library with the numbers of the interface pins
LiquidCrystal lcd(12, 11, 5, 4, 3, 2);

// setup the manual digital input pin


// setup the buttons pins
const int StartPin 		= 6;
const int ModePin 		= 7;
const int PlusPin         = 8;
const int MinusPin         = 9;

// setup the foot pedal pin
const int FootPedal 			= 10;

// setup the impuls servo pin
const int ServoPin			= 13;

// setup the servo
Servo impulsServo;

// setup global variables
int mode = 1;			// stores the current mode number
long interval = 1000; 		// startup nudge interval in milliseconds
long intervalOffset = 0; 	// offset of the nudge interval, if program logic takes longer than expected, use this to correct the interval, in milliseconds
long  intervalIncr = 100;		// if the interval buttons are pressed, the interval gets incremented or decremented by this value
long premillis = 0;
boolean nudge = false;		// initial state of operation in mode 2
int nudgeInt = 0;
char defDispl[17] = "";         // standard display string
int statusbarValue = 0;         // initial value of the statusbar
int servoMin = 90;		// the servo's default (return) position
int servoMax = 120;             // the servo's position where it nudges the shell
int servoWaitTime = 30;		// time between a servo max and a servo min call, in milliseconds
int welcMsgWaitTime = 2500;	// timespan for which the welcome message is displayed, in milliseconds
int buttonPressedWait = 250;    // time to wait after a button is pressed, in ms
int modeButtonWait = 500;       // time to wait, if the mode button was pressed, in ms

int pos = 90;

// setup the whole system
void setup()
{
	// initialize the button pins as input
	pinMode(StartPin, INPUT);
	pinMode(ModePin, INPUT);
	pinMode(PlusPin, INPUT);
	pinMode(MinusPin, INPUT);
	
	// initialize the foot pedal pin
	pinMode(FootPedal, INPUT);
	
	// initialize the impuls start pin
	pinMode(TTLIn, INPUT);
	
	// initialize the impuls output pin as output
	pinMode(TTLOut, OUTPUT);
	
	// attach the servo to its pin
	impulsServo.attach(ServoPin);
	// reset the servo
	impulsServo.write(servoMin);
	
	// set up the LCD's number of columns and rows: 
	lcd.begin(16, 2);
	// print welcome message
	lcd.setCursor(0, 0);
        lcd.print("-= WormNudger =-");
	lcd.setCursor(0, 1);
        lcd.print("      v0.1      ");

	// wait some time, so the welcome message can be read
	delay(welcMsgWaitTime);

}

// the main loop
void loop()
{
    displayDefaults();
	checkModeButton();
	switchToMode();
}


// check, if the mode button was pressed
void checkModeButton()
{
	// the mode button cycles through the 4 modes (1 to 4)
	if (digitalRead(ModePin) == HIGH) 
	{
		mode = (mode % 2) + 1;
                delay(modeButtonWait);
	}
}

// depending on the mode, switch to the various methods
void switchToMode()
{
	switch (mode) 
	{
		case 1: // 1. Automatic interval impuls
			autoInterval();
			break;
		
		case 2: // 2. Automatic interval impuls manual started
			autoIntervalManuStart();
			break;
			
		case 3: // 3. Manual foot pedal triggered impuls
			manuFootPedal();
			break;
			
		case 4: // 4. Manual input triggered impuls
			manuDigiInput();
			break;
			
		default: // 1. Automatic interval impuls
			autoInterval();
			break;
	}
	displayDefaults();
}

// The impuls for the servo starts automatically  and repeats in an adjustable interval
void autoInterval()
{
	checkIntervalButtons();
	triggerImpuls();
	sleepForInterval();
}

// The impuls is started manual by an digital input signal und repeats henceforward in an adjustable interval
void autoIntervalManuStart()
{
	checkIntervalButtons();
	
	// check if the digital input triggers the start
	if (digitalRead(TTLIn) == HIGH)
	{
		// toggle the nudge state 
		if (nudge == true) 
		{
			nudge = false;
		}
		if (nudge == false)
		{
			nudge = true;
		}

                nudgeInt = (nudgeInt + 1) % 2;
                delay(250);
	}
	
	// if the nudge state is set to true: nudge!
	//if (nudge)
        if (nudgeInt == 1);
        {
		triggerImpuls();
		sleepForInterval();
	}
}

// The impuls is triggered manual by a foot pedal
void manuFootPedal()
{
	if (digitalRead(FootPedal) == HIGH)
	{
		triggerImpuls();
	}
}

// The impuls is triggered manual by an digital input signal
void manuDigiInput()
{
	if (digitalRead(StartPin) == HIGH)
	{
		triggerImpuls();
	}
}

// check, if an interval button is pressed and de- or increase the interval
void checkIntervalButtons()
{
	// "+"-button pressed: increase the interval
	if (digitalRead(PlusPin) == HIGH)
	{
		interval += intervalIncr;
                delay(buttonPressedWait);
	}
	
	// "-"-button pressed: decrease the interval
	if (digitalRead(MinusPin) == HIGH)
	{
		interval -= intervalIncr;
                delay(buttonPressedWait);
	}
	displayDefaults();
}

// trigger the servo for an impuls
void triggerImpuls()
{
	
 
	// set the TTLOut to high
	digitalWrite(TTLOut, HIGH);
	
	for(pos = 70; pos < 120; pos += 1)  // goes from 0 degrees to 180 degrees 
  {                                  // in steps of 1 degree 
    impulsServo.write(pos);              // tell servo to go to position in variable 'pos' 
    delay(3);                       // waits 15ms for the servo to reach the position 
  } 
  for(pos = 120; pos>=71; pos-=1)     // goes from 180 degrees to 0 degrees 
  {                                
    impulsServo.write(pos);              // tell servo to go to position in variable 'pos' 
    delay(3);                       // waits 15ms for the servo to reach the position 
  } 
	// set the TTLOut to low
	digitalWrite(TTLOut, LOW);
	unsigned long currentMillis = millis();
}

// wait for the set interval, meanwhile check the buttons
void sleepForInterval()
{
	int correctedInterval = interval + intervalOffset;
	int initialMode = mode;
	int initialInterval = interval;
	for (int i=0; i<correctedInterval; i++)
	{
		// check, if mode or interval has changed
		checkModeButton();
		checkIntervalButtons();
		
		// if something (mode, interval) was changed, leave the loop = don't wait any longer
		if ((mode != initialMode) || (interval != initialInterval))
		{
			break;
		}

		// wait for 1ms
		delay(1);
	}
}


// default display with mode and interval settings
void displayDefaults()
{
        // get the initial display string
        char initDisplay[17] = "";
        strcpy(initDisplay, defDispl);
        
        // construct the default display string line 1
        char defaultDisplay1[17] = "";
        strcat(defaultDisplay1, "M:");       
        char modeStr[2];
        sprintf(modeStr, "%d", mode);        
        strcat(defaultDisplay1, modeStr);
        strcat(defaultDisplay1, " ");
        strcat(defaultDisplay1, "W:");
        char intervalStr[6];
        sprintf(intervalStr, "%d", interval);
        strcat(defaultDisplay1, intervalStr);    
        strcat(defaultDisplay1, " ms");
		// fill with spaces
        for (int i=0; i<16-strlen(defaultDisplay1+1); i++)
        {
              strcat(defaultDisplay1, " ");
        }
        // construct the default display string line 2
        char defaultDisplay2[17] = "";
        strcat(defaultDisplay2, "I:");       
        char modeStr[2];
        sprintf(modeStr, "%d", interval);        
        strcat(defaultDisplay2, modeStr);
        strcat(defaultDisplay2, " ms ");
        strcat(defaultDisplay2, "R:");
        char intervalStr[6];
        sprintf(intervalStr, "%d", repeat);
        strcat(defaultDisplay2, intervalStr);    
        strcat(defaultDisplay2, " x");
        // fill with spaces
        for (int i=0; i<16-strlen(defaultDisplay2+1); i++)
        {
              strcat(defaultDisplay2, " ");
        }

		
        // only refresh display when something changed
        if (strcmp(defaultDisplay1, initDisplay1) != 0) 
        { 
          lcd.setCursor(0, 0);     
          lcd.print(defaultDisplay1);
          strcpy(defDispl, defaultDisplay1);
        }
		  if (strcmp(defaultDisplay2, initDisplay2) != 0) 
        { 
          lcd.setCursor(0, 1);     
          lcd.print(defaultDisplay2);
          strcpy(defDispl, defaultDisplay2);
        }
}
